#include "RFIDNFCLib.h"
#include "BLE112Lib.h"
#include <Time.h>
#include <TimeAlarms.h>



RFIDNFCLib RFIDNFC;
BLE112Lib BLE;
bool tag_readed = false;
bool connect = false;
bool _reset_BLE112 = false;
bool _activate_heart_rate_BLE112 = false;
bool _mote_discovered = false;
bool _hay_movil = true;
bool _mobile_connected = false;
bool _established = false;
bool _tx_now = true;

bool primera_vez = true;

int estado = 0;
int paso = 1;
//uint8 cadena[29];
uint8 valor_altura_pesa = 0;
//uint8 *valor_info_general = cadena;
int atributo_altura_pesa = 17;
int atributo_general_info = 22;

double millis_ant = 0;
double millis_new = 0;

int contador = 0;

bool BLE_started = false;



void setup(){

	//setTime(8,29,0,1,1,11); // set time to Saturday 8:29:00am Jan 1 2011

		
	// UART for NFC/RFID module
	Serial1.begin(115200);
	// UART for Bluetooth Smart module
	Serial2.begin(28800);
	// UART for ZigBee/802.15.4 module
	Serial3.begin(9600);

	// UART for debugging purpouse and LCD Screen (future)
	Serial.begin(115200);
	Serial.println("Sensor GYM Mote");


	Serial.println("-->\tStartBLE112");
	BLE.StartBLE112();
	//pinMode(RxDSerial, INPUT);
	//pinMode(TxDSerial, OUTPUT);

	/*for(int i=0; i<28; i++)
	{
		cadena[i] = ' ';
	}*/
	
	/*Serial.println("-->\tStartRFIDNFC");
	RFIDNFC.StartRFIDNFC();
	Serial.println("-->\tConfigureSAM");
	RFIDNFC.ConfigureSAM();*/
	
	//delay(1000);
    //RFIDPortDebug.begin(9600);
	//RFIDPort.listen();

	//!It is needed to launch a simple command to sycnchronize
	//getFirmware();
	//configureSAM();
}

void loop(){
	BLE.CheckActivityBLE112();
	//Alarm.delay(1);
	
	millis_new = millis();
	if(millis_new - millis_ant > 500){
		_tx_now = true;
		millis_ant = millis_new;
		
	}

	
		
	if(!tag_readed){
		/*Serial.println("-->\tStartRFIDNFC");
		RFIDNFC.StartRFIDNFC();
		Serial.println("-->\tConfigureSAM");
		RFIDNFC.ConfigureSAM();
		Serial.println("-->\tDetectTag");
		RFIDNFC.DetectTag();
		RFIDNFC.PrintUIDTagDetected();
		Serial3.println("-->\tUsuario Comienza Ejercicio");*/
		
		tag_readed = true;
		estado = 0;
		//Alarm.timerOnce(100, ResetWaitingTag);
	}
	else if (_hay_movil) {
		
		switch(estado){
		
		case 0: 
			Serial.println("-->\tReset");
			BLE.Reset();
			if(BLE.IsConnected()){
				estado = 4;
			}
			Alarm.delay(5);
			estado++;
		break;
		
		case 1:
			if(BLE.IsReseted()){
				Serial.println("-->\tSetAdv");
				BLE.SetAdvParameters(32,48,7);
				estado++;
			}
			else{
				estado--;
			}
			Alarm.delay(5);

		break;

		case 2:
			if(BLE.IsAdvCorrect()){
				Serial.println("-->\tSetMode");
				BLE.SetMode(BGLIB_GAP_DISCOVER_GENERIC,BGLIB_GAP_UNDIRECTED_CONNECTABLE);
				//Alarm.timerOnce(10, CheckConnection);
				Alarm.delay(50);
				estado++;
			}
			else{
				Alarm.delay(50);
				estado--;
			}
		break;

		case 3:
			
			//BLE.ConnectToHearRateMonitor();
			//BLE.ConnectToMobile();
			//estado++;
						
			if(BLE.IsConnected()){
				primera_vez=true;
				WriteGeneralInfo();
				Alarm.delay(100);
				BLE.SetAdvParameters(1600,4000,7);//1600,4000,7
				Alarm.delay(50);
				BLE.SetMode(BGLIB_GAP_NON_DISCOVERABLE,BGLIB_GAP_NON_CONNECTABLE);
				Alarm.delay(50);
				estado++;
			}
		break;

		case 4:
			if(_tx_now && BLE.IsConnected()){
				//Alarm.timerOnce(1, WriteLocalValue);
				
				
				if(primera_vez){
					WriteGeneralInfo();
					contador++;
					if(contador>2)
					{
						contador=0;
						primera_vez=false;
					}
				}
				else{
					WriteLocalValue();
					//primera_vez = true;
					
				}

				_tx_now = false;
			}
			else if (BLE.IsDisconnected()){
				estado=0;
				_tx_now = true;
			}
		break;

		case 5:
			BLE.Disconect();
			estado=6;

		break;

			
			
		default:
			break;

		}


	}

}
	
	


void ResetWaitingTag(){
	Serial.println("-->\tReseting tag_readed...");
	tag_readed = false;
}

void ReadHearRate(){
	Serial.print("###\tHear Rate: { ");Serial.print((uint8_t)BLE.GetHeartRate());
	
	Serial.println(" }");
}

void WriteLocalValue(){
	Serial.println("-->\tWrite Local Attribute: ");
	static bool subiendo = true;
	if(subiendo){
		valor_altura_pesa++;
		if(valor_altura_pesa>9) subiendo = false;
	}
	else {
		valor_altura_pesa--;
		if(valor_altura_pesa==0) subiendo = true;
	}
	BLE.Write_Local_Attribute(atributo_altura_pesa, &valor_altura_pesa);
	//Alarm.timerOnce(1, WriteLocalValue);
	//_tx_now = true;
}

void WriteGeneralInfo(){
	Serial.println("-->\tWrite General Info Attribute: ");
	
	uint8 cadena[20] = "Polea Alta         ";
	cadena[16]=2;	// Serie Actual
	cadena[17]=3;	// Series totales
	cadena[18]=35;	// Peso 
	cadena[19]=12;	// Repeticiones
	BLE.Write_Local_Attribute(atributo_general_info, &cadena[0]); ///valor_info_general


	//Alarm.timerOnce(1, WriteLocalValue);
	//_tx_now = true;
}

void CheckConnection(){
	//Serial.print("###\tCheck Connection: ");
	switch(paso){
	case 1:
		if(!BLE.IsConnected()){
			BLE.SetMode(BGLIB_GAP_NON_DISCOVERABLE,BGLIB_GAP_NON_CONNECTABLE);
			BLE.SetAdvParameters(1600,4000,7);
			BLE.SetMode(BGLIB_GAP_LIMITED_DISCOVERABLE,BGLIB_GAP_UNDIRECTED_CONNECTABLE);
			paso++;
			//Alarm.timerOnce(10, CheckConnection);
		}
		break;

	case 2:
		if(!BLE.IsConnected()){
			BLE.SetMode(BGLIB_GAP_NON_DISCOVERABLE,BGLIB_GAP_NON_CONNECTABLE);
		}
		break;

	default:
		break;
	}
	
}

void digitalClockDisplay()
{
  // digital clock display of the time
  Serial.print(hour());
  printDigits(minute());
  printDigits(second());
  Serial.println(); 
}

void printDigits(int digits)
{
  Serial.print(":");
  if(digits < 10)
    Serial.print('0');
  Serial.print(digits);
}